function []= draw3DFamilySolution_matrixnorm(F,type,KT)

numsols=200;
width=KT(1,3)*2;
height=KT(2,3)*2;
w=width;
h=height;

Kguess=[width 0 width/2; 0 width height/2; 0 0 1];

if(iscell(F)~=1)
    F={F};
end


sz=size(F,2);
for j=1:sz
    
    if(strcmpi(type, 'foc')==1)
        drawFocal(F{1,j},w,h,numsols,Kguess,KT);
    else
        drawOC(F{1,j},w,h,numsols,Kguess,KT);
    end
    figure
end

end




function []=drawFocal(F,w,h,numsols,Kguess,KT)

numsols=20+numsols;
F=F/norm(F,'fro');
side=300;

focal2start=KT(2,2)-side;
focal2end=KT(2,2)+side;


    step=((focal2end-focal2start)/(numsols-1));
    xarray2=focal2start:step:focal2end;    


focal1start=KT(1,1)-side;
focal1end=KT(1,1)+side;


    step=((focal1end-focal1start)/(numsols-1));
    xarray1=focal1start:step:focal1end;    



f1=F(1,1);
f2=F(1,2);
f3=F(1,3);

f4=F(2,1);
f5=F(2,2);
f6=F(2,3);

f7=F(3,1);
f8=F(3,2);
f9=F(3,3);


xc=KT(1,3);
yc=KT(2,3);


obj=zeros(numsols,numsols);
for j=1:numsols
    for i=1:numsols
        
        foc2=xarray2(i);
        foc1=xarray1(j);
        
        obj(i,j)=2*(foc1^2*(f7 + f1*xc + f4*yc)^2 + foc2^2*(f8 + f2*xc + f5*yc)^2 + (f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)^2)^2 + 4*(f1*f4*foc1^3*foc2 + f2*f5*foc1*foc2^3 + (f3 + f1*xc + f2*yc)*(f6 + f4*xc + f5*yc)*foc1*foc2)^2 + 2*(foc1^2*(f3 + f1*xc + f2*yc)^2 + f1^2*foc1^4 + f2^2*foc1^2*foc2^2)^2 + 2*(foc2^2*(f6 + f4*xc + f5*yc)^2 + f5^2*foc2^4 + f4^2*foc1^2*foc2^2)^2 + 4*(f1*(f7 + f1*xc + f4*yc)*foc1^3 + f2*(f8 + f2*xc + f5*yc)*foc1*foc2^2 + (f3 + f1*xc + f2*yc)*(f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)*foc1)^2 + 4*(f4*(f7 + f1*xc + f4*yc)*foc1^2*foc2 + f5*(f8 + f2*xc + f5*yc)*foc2^3 + (f6 + f4*xc + f5*yc)*(f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)*foc2)^2 - (foc1^2*(f3 + f1*xc + f2*yc)^2 + foc1^2*(f7 + f1*xc + f4*yc)^2 + foc2^2*(f6 + f4*xc + f5*yc)^2 + foc2^2*(f8 + f2*xc + f5*yc)^2 + (f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)^2 + f1^2*foc1^4 + f5^2*foc2^4 + f2^2*foc1^2*foc2^2 + f4^2*foc1^2*foc2^2)^2;
        
    end
end
[Xm,Ym] = meshgrid(xarray1,xarray2);

figure
meshc(Xm,Ym,obj);
hold on
title({['objective function for the two focal lengths']});
xlabel('focal length 1');
ylabel('focal length 2');
zlabel('objective function value');
plot3(KT(1,1),KT(2,2),0, 'marker','o','markersize',10,'markeredgecolor',[1,0,0],'markerfacecolor',[1,0,0]);
hold off


figure
[sols1,xs1]= provideFamilySolution_matrixnorm(F,w,h,2,numsols,Kguess,'foc1');
hold on 
plot(xs1,sols1);
plot(KT(2,2),KT(1,1), 'marker','o','markersize',10,'markeredgecolor',[1,0,0],'markerfacecolor',[1,0,0]);
title({['objective function for foc1']});
xlabel('focal length 2');
ylabel('focal length 1');
hold off

figure
[sols2,xs2]= provideFamilySolution_matrixnorm(F,w,h,2,numsols,Kguess,'foc2');
plot(xs2,sols2);
hold on 
plot( KT(1,1),KT(2,2),'marker','o','markersize',10,'markeredgecolor',[1,0,0],'markerfacecolor',[1,0,0]);
title({['objective function for foc1']});
xlabel('focal length 1');
ylabel('focal length 2');
hold off

figure
mesh(xarray1,xarray2,obj);
hold on
plot3(KT(1,1),KT(2,2),0, 'marker','o','markersize',10,'markeredgecolor',[1,0,0],'markerfacecolor',[1,0,0]);
plot3(xs2,sols2,zeros(size(xs2,1),1),sols1,xs1,zeros(size(xs1,1),1));
title({['objective function for the two focal lengths']});
xlabel('focal length 1');
ylabel('focal length 2');
zlabel('objective function value adjusted');
hold off

figure

obj2=obj;
t=0.05;
obj2(obj>t)=t*100;
mesh(xarray1,xarray2,obj2);
hold on
title({['objective function for the two focal lengths']});
xlabel('focal length 1');
ylabel('focal length 2');
zlabel('objective function value adjusted');
plot3(xs2,sols2,zeros(size(xs2,1),1),sols1,xs1,zeros(size(xs1,1),1));
plot3(KT(1,1),KT(2,2),0, 'marker','o','markersize',10,'markeredgecolor',[1,0,0],'markerfacecolor',[1,0,0]);
hold off


figure
hold on

mi=min(min(obj));
stecp=max(range(obj))/200;
v=[(0:stecp:(mi+(5*stecp))) ];
[C,h]=contour(Xm,Ym,(obj),v);
axis square
clabel(C,h)    
plot( KT(1,1),KT(2,2),'marker','o','markersize',10,'markeredgecolor',[1,0,0],'markerfacecolor',[1,0,0]);
%plot(xs2,sols2,sols1,xs1);
hold off


end



function []=drawOC(F,w,h,numsols,Kguess,KT)

F=F/norm(F,'fro');



xcstart=max(20,(-w/4)+Kguess(1,3))-10;
xcend=min(w/2+w/4 ,(w/4)+Kguess(1,3))+10;

ycstart=max(20,(-h/4)+Kguess(2,3))-10;
ycend=min(h/2+h/4 ,(h/4)+Kguess(2,3))+10;

numsols=20+numsols;
if(numsols==1)
    xarray=[Kguess(2,2)];
else
    step=((xcend-xcstart)/(numsols-1));
    xarray=xcstart:step:xcend;
    
    step=((ycend-ycstart)/(numsols-1));
    yarray=ycstart:step:ycend;
    
end



f1=F(1,1);
f2=F(1,2);
f3=F(1,3);

f4=F(2,1);
f5=F(2,2);
f6=F(2,3);

f7=F(3,1);
f8=F(3,2);
f9=F(3,3);

foc1=Kguess(1,1);
foc2=Kguess(2,2);



obj=zeros(numsols,numsols);
for j=1:numsols
    for i=1:numsols
        
        yc=yarray(i);
        xc=xarray(j);
        
        obj(i,j)=2*(foc1^2*(f7 + f1*xc + f4*yc)^2 + foc2^2*(f8 + f2*xc + f5*yc)^2 + (f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)^2)^2 + 4*(f1*f4*foc1^3*foc2 + f2*f5*foc1*foc2^3 + (f3 + f1*xc + f2*yc)*(f6 + f4*xc + f5*yc)*foc1*foc2)^2 + 2*(foc1^2*(f3 + f1*xc + f2*yc)^2 + f1^2*foc1^4 + f2^2*foc1^2*foc2^2)^2 + 2*(foc2^2*(f6 + f4*xc + f5*yc)^2 + f5^2*foc2^4 + f4^2*foc1^2*foc2^2)^2 + 4*(f1*(f7 + f1*xc + f4*yc)*foc1^3 + f2*(f8 + f2*xc + f5*yc)*foc1*foc2^2 + (f3 + f1*xc + f2*yc)*(f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)*foc1)^2 + 4*(f4*(f7 + f1*xc + f4*yc)*foc1^2*foc2 + f5*(f8 + f2*xc + f5*yc)*foc2^3 + (f6 + f4*xc + f5*yc)*(f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)*foc2)^2 - (foc1^2*(f3 + f1*xc + f2*yc)^2 + foc1^2*(f7 + f1*xc + f4*yc)^2 + foc2^2*(f6 + f4*xc + f5*yc)^2 + foc2^2*(f8 + f2*xc + f5*yc)^2 + (f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)^2 + f1^2*foc1^4 + f5^2*foc2^4 + f2^2*foc1^2*foc2^2 + f4^2*foc1^2*foc2^2)^2;
        
    end
end

mesh(xarray,yarray,obj);

title({['objective function for the two focal lengths']});
xlabel('xc');
ylabel('yc');
zlabel('objective function value');
figure

obj(obj<0.01)=-100;
figure
mesh(xarray,xarray,obj);
title({['objective function for the two focal lengths']});
xlabel('focal length 2');
ylabel('focal length 1');
zlabel('objective function value adjusted');

figure
[sols1,xs1]= provideFamilySolution_matrixnorm(F,w,h,2,numsols,Kguess,'yc');
plot(xs1,sols1);
title({['objective function for yc']});
xlabel('xc');
ylabel('yc');
figure
[sols2,xs2]= provideFamilySolution_matrixnorm(F,w,h,2,numsols,Kguess,'xc');
plot(xs2,sols2);
title({['objective function for xc']});
ylabel('xc');
xlabel('yc');

end


%
% this is how you generate this formula
% syms f1 f2 f3 f4 f5 f6 f7 f8 f9 foc1 foc2 xc yc real
% K=[foc1 0 xc ; 0 foc2 yc; 0 0 1]
% F=[f1 f2 f3 ; f4 f5 f6 ; f7 f8 f9 ]
% G=(K')*F*K
% GGT=G*(G')
% v=(2*trace(GGT*GGT))-((trace(GGT))*(trace(GGT)))
% vs=simplify(v)
% R = collect(vs ,foc1) to collect terms for foc1
